// Fill out your copyright notice in the Description page of Project Settings.

#include "WorldRefLineSpline.h"

void FWorldRefLineSpline::Add(const glm::dvec3& param, const glm::dvec3& p0, const glm::dvec3& m0, const glm::dvec3& p1, const glm::dvec3& m1)
{
	Add({ param, p0, m0, p1, m1 });
}

void FWorldRefLineSpline::Add(const Interval& interval)
{
	_spline.push_back(interval);
}

bool FWorldRefLineSpline::IsEmpty() const
{
	return _spline.empty();
}

int FWorldRefLineSpline::IntervalCount() const
{
	return static_cast<int>(_spline.size());
}

double FWorldRefLineSpline::Length() const
{
	if (!IsEmpty())
	{
		double lastS = _spline.back().Param.y;
		double lastLength = _spline.back().Param.z;
		double firstS = _spline.front().Param.y;
		return lastS + lastLength - firstS;
	}
	return 0.0;
}

void FWorldRefLineSpline::UpdateReparam(int intervalSteps)
{
	int intervalCount = static_cast<int>(_spline.size());

	_reparam.clear();
	_reparam.reserve(intervalCount * intervalSteps + 1);

	double lengthSum = 0.0;
	for (int i = 0; i < intervalCount; i++)
	{
		for (int j = 0; j < intervalSteps; j++)
		{
			// Compute the length of [0, t] in i-th interval.
			double t = static_cast<double>(j) / intervalSteps;
			double length = (j == 0) ? 0.0 : GetIntervalLength(i, t);
			_reparam.push_back({ lengthSum + length, i + t });
		}
		lengthSum += GetIntervalLength(i, 1.0);
	}
	_reparam.push_back({ lengthSum, static_cast<double>(intervalCount) });
}

double FWorldRefLineSpline::GetIntervalLength(int index, double t) const
{
	// Reference: https://en.wikipedia.org/wiki/Gaussian_quadrature
	// Evaluate the length of a Hermite spline segment.
	// This calculates the integral of |dP/dt| dt, where P(t) is the spline equation with components (x(t), y(t), z(t)).
	// This isn't solvable analytically, so we use a numerical method (Legendre-Gauss quadrature) which performs very well
	// with functions of this type, even with very few samples.  In this case, just 5 samples is sufficient to yield a
	// reasonable result.
	struct LegendreGaussCoefficient
	{
		double Abscissa;
		double Weight;
	};

	static const LegendreGaussCoefficient LegendreGaussCoefficients[] =
	{
		{  0.0,											128.0/225.0 },
		{ -std::sqrt(5.0-2.0*std::sqrt(10.0/7.0))/3.0,	(322.0+13.0*std::sqrt(70.0))/900.0 },
		{  std::sqrt(5.0-2.0*std::sqrt(10.0/7.0))/3.0,	(322.0+13.0*std::sqrt(70.0))/900.0 },
		{ -std::sqrt(5.0+2.0*std::sqrt(10.0/7.0))/3.0,	(322.0-13.0*std::sqrt(70.0))/900.0 },
		{  std::sqrt(5.0+2.0*std::sqrt(10.0/7.0))/3.0,	(322.0-13.0*std::sqrt(70.0))/900.0 }
	};

	glm::dvec3 P0 = _spline[index].P0;
	glm::dvec3 M0 = _spline[index].M0;
	glm::dvec3 P1 = _spline[index].P1;
	glm::dvec3 M1 = _spline[index].M1;

	// The length here is 2D length. Elevation is not considered in OpenDRIVE.
	P0.z = M0.z = P1.z = M1.z = 0.0;

	glm::dvec3 coeff1 = ((P0 - P1) * 2.0 + M0 + M1) * 3.0;
	glm::dvec3 coeff2 = (P1 - P0) * 6.0 - M0 * 4.0 - M1 * 2.0;
	glm::dvec3 coeff3 = M0;

	double tHalf = t * 0.5;
	double length = 0.0;

	for (const auto& LegendreGaussCoefficient : LegendreGaussCoefficients)
	{
		// Calculate derivative at each Legendre-Gauss sample, and perform a weighted sum.
		double alpha = tHalf * (1.0 + LegendreGaussCoefficient.Abscissa);
		glm::dvec3 derivative = (coeff1 * alpha + coeff2) * alpha + coeff3;
		length += glm::length(derivative) * LegendreGaussCoefficient.Weight;
	}
	length *= tHalf;

	return length;
}

int FWorldRefLineSpline::FindInterval(double t) const
{
	if (IsEmpty() || t < _spline[0].Param.x)
	{
		return -1;
	}

	int last = static_cast<int>(_spline.size() - 1);
	if (t >= _spline[last].Param.x)
	{
		return last;
	}

	Interval greaterThan;
	greaterThan.Param= glm::dvec3(t, 0.0, 0.0);
	auto it = std::upper_bound(_spline.begin(), _spline.end(), greaterThan,
		[](const Interval& a, const Interval& b) -> bool
		{
			return a.Param.x < b.Param.x;
		});
	return static_cast<int>(it - _spline.begin() - 1);
}

int FWorldRefLineSpline::FindIntervalAtDistance(double s) const
{
	if (IsEmpty() || s < _reparam[0].S)
	{
		return -1;
	}

	int last = static_cast<int>(_reparam.size() - 1);
	if (s >= _reparam[last].S)
	{
		return last;
	}

	Reparam greaterThan;
	greaterThan.S = s;
	auto it = std::upper_bound(_reparam.begin(), _reparam.end(), greaterThan,
		[](const Reparam& a, const Reparam& b) -> bool
		{
			return a.S < b.S;
		});
	return static_cast<int>(it - _reparam.begin() - 1);
}

double FWorldRefLineSpline::FindParamAtDistance(double s) const
{
	// Binary search to find index of lower bound of input value.
	int index = FindIntervalAtDistance(s);

	// If before the first point, return its value.
	if (index == -1)
	{
		return !IsEmpty() ? _reparam[0].T : 0.0;
	}

	// If on or beyond the last point, return its value.
	int last = static_cast<int>(_reparam.size() - 1);
	if (index == last)
	{
		return _reparam[last].T;
	}

	// Somewhere within curve range - interpolate.
	const auto& prev = _reparam[index];
	const auto& next = _reparam[index + 1];

	double diff = (next.S - prev.S);
	if (diff > DBL_EPSILON)
	{
		double alpha = (s - prev.S) / diff;
		return glm::mix(prev.T, next.T, alpha);
	}
	else
	{
		return _reparam[index].T;
	}
}

glm::dvec3 FWorldRefLineSpline::Eval(double t) const
{
	// Binary search to find index of lower bound of input value.
	int index = FindInterval(t);

	// If before the first point, return its value.
	if (index == -1)
	{
		return !IsEmpty() ? _spline[0].P0 : glm::dvec3(0.0);
	}

	// If on or beyond the last point, return its value.
	int last = static_cast<int>(_spline.size() - 1);
	if (index == last && t >= _spline[last].Param.x + 1.0)
	{
		return _spline[last].P1;
	}

	// Somewhere within curve range - interpolate.
	const auto& interval = _spline[index];
	double alpha = t - interval.Param.x;

	return FWorldMath::HermiteInterp(interval.P0, interval.M0, interval.P1, interval.M1, alpha);
}

glm::dvec3 FWorldRefLineSpline::EvalAtDistance(double s) const
{
	double t = FindParamAtDistance(s);
	return Eval(t);
}

glm::dvec3 FWorldRefLineSpline::EvalTangent(double t) const
{
	// Binary search to find index of lower bound of input value.
	int index = FindInterval(t);

	// If before the first point, return its tangent value
	if (index == -1)
	{
		return !IsEmpty() ? _spline[0].M0 : glm::dvec3(1.0, 0.0, 0.0);
	}

	// If on or beyond the last point, return its tangent value.
	int last = static_cast<int>(_spline.size() - 1);
	if (index == last && t >= _spline[last].Param.x + 1.0)
	{
		return _spline[last].M1;
	}

	// Somewhere within curve range - interpolate.
	const auto& interval = _spline[index];
	double alpha = t - interval.Param.x;

	return FWorldMath::HermiteDerivative(interval.P0, interval.M0, interval.P1, interval.M1, alpha);
}

glm::dvec3 FWorldRefLineSpline::EvalTangentAtDistance(double s) const
{
	double t = FindParamAtDistance(s);
	return EvalTangent(t);
}

const std::vector<FWorldRefLineSpline::Interval>& FWorldRefLineSpline::Get() const
{
	return _spline;
}

std::vector<FWorldRefLineSpline::Interval>& FWorldRefLineSpline::Get()
{
	return _spline;
}

FWorldRefLineSpline::Ptr FWorldRefLineSpline::Clone() const
{
	return std::make_shared<FWorldRefLineSpline>(*this);
}
